package environment;

import java.awt.Color;
import java.awt.Graphics;
import java.awt.Point;
import java.awt.Polygon;
import java.awt.Rectangle;
import java.util.ArrayList;

/**
 *
 * @author Toar
 */
public class Box implements EnvObjInterface
{
    private String id;
	private boolean isPushable = true;
	protected boolean isBeingPushed = false;
    private Capabilities capabilitiesNeeded;
    protected int numberOfRobotsToPush = 2;
	protected long round1 = -1, round2 = -1, round3 = -1;
    
	/** TOAR: box dimension **/
    private int width=120;
    private int height=50;
	private int x;			// center x
	private int y;			// center y
	double angle = 90;

	/** TODO: bounds need to be changed to Polygon **/
	private Rectangle bounds;
	private Polygon polyBounds;
	private Color boxColor = new Color(100,0,0);
	private Color boxColor23 = Color.PINK;
	private Color boxColor34 = Color.CYAN;
	private Color boxColor41 = Color.GREEN;
	
	/** Pushing and rotation stuffs - from Dr. Ying's simulator
	 * 
	 *    __1__6__2__
	 *   | 			 |
	 *   0	     	 3
	 *   |__5__7__4__|
	 *  
	 * */
	private double forceValue[]=new double[8];
	private double netForceAngle=0;
    private double netForceValue=0;
    private double netTorque=0;
    private double lArm=20;  // force arm
    private double Mf=0.2; // Force-Displacement factor
    private double Mt=0.02; //Torque-Angle factor    

    
    /** Corner points - accessible by Robot **/
    protected Point p1;
    protected Point p2;
    protected Point p3;
    protected Point p4;
    
    // 0 default, 1 and above is configurable, see draw()
	public int drawType = 0;
    
    /** Goal path **/
    protected ArrayList<Goal> goalPath = null;
    
    /** Trace **/
    private ArrayList<BoxPosition> traceList = new ArrayList<BoxPosition>();
    
	/**
	 * Given center of the box
	 * @param x
	 * @param y
	 */
	public Box(String myID, int x, int y) 
    {
        id=myID;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		this.capabilitiesNeeded = new Capabilities(true,true,true,true,true);
    }
	
	/**
	 * Given center of the box, width and height
	 * @param x
	 * @param y
	 */
	public Box(String myID, int x, int y, int width, int height) 
    {
        id=myID;
        this.width = width;
        this.height = height;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		this.capabilitiesNeeded = new Capabilities(true,true,true,true,true);
    }
	
	public Box(String myID, int x, int y, int width, int height, double angle) 
    {
        id=myID;
        this.width = width;
        this.height = height;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		this.angle = angle;
		this.capabilitiesNeeded = new Capabilities(true,true,true,true,true);
    }
	
	/**
	 * Given center of the box
	 * @param x
	 * @param y
	 * @param attributesNeeded - attributes needed to push this box
	 */
	public Box(String myID, int x, int y, Capabilities capabilitiesNeeded) 
    {
        id=myID;
		this.bounds = new Rectangle(x,y,width,height);
		this.x = (int)bounds.getCenterX();
		this.y = (int)bounds.getCenterY();
		this.capabilitiesNeeded = capabilitiesNeeded;
    }
	
	public double getWidth() {
		return width;
	}

	public double getHeight() {
		return height;
	}

	/**
	 * Returns global coordinate for center x of robot
	 */
	public int getX() {
		return this.x;
	}

	/**
	 * Returns global coordinate for center y of robot
	 */
	public int getY() {
		return this.y;
	}
	
	public double getAngle() {
		return angle;
	}
	
	public void setAngle(double angle) {
		this.angle = angle;
	}

	public boolean isPushable() {
		return isPushable;
	}
	
	public void setPushable(boolean b){
		isPushable = b;
	}
	
	public void setNumberOfRobotsToPush(int n)
	{
		this.numberOfRobotsToPush = n;
	}

	public void move(int howManyX, int howManyY, ArrayList<EnvObjInterface> a)
	{
		this.x += howManyX;
		this.y += howManyY;
		this.bounds.setBounds(x, y, width, height);
	}
	
	/** @deprecated **/
	public boolean isTouching(int x, int y) 
	{
		return this.bounds.contains(x, y);
	}

	public boolean isTouching(EnvObjInterface eObj) 
	{
		return polyBounds.intersects(eObj.getBounds());
		//return this.bounds.intersects(eObj.getBounds());
	}
	
	public void printStats() {
		System.out.println("Box " + id + " is at " + this.x + ", " + this.y);
	}

	public Rectangle getBounds() {
		return polyBounds.getBounds();
	}

	public void setColor(Color c)
	{
		boxColor = c;
	}
	
	public void drawTrace(Graphics g)
	{
		for(BoxPosition bp : traceList)
		{
			Point corner1 = new Point(-width/2,-height/2);
			corner1 = frame2To1(corner1, bp);
	        
			Point corner2 = new Point(width/2,-height/2);
			corner2 = frame2To1(corner2, bp);
	        
			Point corner3 = new Point(width/2,height/2);
			corner3 = frame2To1(corner3, bp);

	        Point corner4 = new Point(-width/2,height/2);
	        corner4 = frame2To1(corner4, bp);

	        g.setColor(boxColor);
	        
	        g.drawLine( corner1.x, corner1.y, corner2.x, corner2.y);
	        g.drawLine( corner2.x, corner2.y, corner3.x, corner3.y);
	        g.drawLine( corner3.x, corner3.y, corner4.x, corner4.y);
	        g.drawLine( corner4.x, corner4.y, corner1.x, corner1.y);
		}
	}
	
	public void draw(Graphics g)
	{
		drawTrace(g);
		updateCorners();
		
		g.setColor(boxColor);
        
        g.drawLine( p1.x, p1.y, p2.x, p2.y);
        g.setColor(boxColor23);
        g.drawLine( p2.x, p2.y, p3.x, p3.y);
        g.setColor(boxColor34);
        g.drawLine( p3.x, p3.y, p4.x, p4.y);
        g.setColor(boxColor41);
        g.drawLine( p4.x, p4.y, p1.x, p1.y);
        
        Polygon p = new Polygon();
        
        p.addPoint(p1.x, p1.y);
        p.addPoint(p2.x, p2.y);
        p.addPoint(p3.x, p3.y);
        p.addPoint(p4.x, p4.y);
        
        //g.setColor(Color.CYAN);
        //g.fillRect((int)p.getBounds2D().getMinX(), (int)p.getBounds2D().getMinY(), (int)p.getBounds2D().getWidth(), (int)p.getBounds2D().getHeight());
        
        g.setColor(boxColor);
        if(drawType == 0) g.fillPolygon(p);
        
        
        g.setColor(Main.stringColor);
        g.drawString(id, (int)x, (int)y);
        
        //drawGoal(g);
	}
	
	private void drawGoal(Graphics g)
	{
		if(this.goalPath == null) return;
		Point points[] = getStartingPoints();
		g.setColor(Color.red);
		int curX = x, curY = y;
		int buffX, buffY;
		int buffX2, buffY2;
		double angle;
		int counter = 1;
		for(Goal goal: goalPath)
		{
			/** Depricated - will come back to this later
			  	abigail			 
			if(counter == 1)
			{
				points = getStartingPoints();
				angle = getGoalAngle(new Point(goal.x, goal.y));
				points = getStartingPoints();
				double err = 5;
				int buffXmult, buffYmult, buffX2mult, buffY2mult;

				if(angle <= 0 + err || angle >= 360 - err)
				{
					buffXmult, buffYmult, buffX2mult, buffY2mult
				}
				else if(angle <= 180 + err && angle >= 180 - err)
				else if(angle <= 90 && angle >= 0)
				else if(angle <= 180 && angle >= 90)
				else if(angle <= 270 && angle >= 180)
				else if(angle >= 270)
				
				buffX =(int)( goal.x + ((width/2)*Math.sin(Math.toRadians(angle)))); 				
				buffY =(int)( goal.y - ((width/2)*Math.cos(Math.toRadians(angle)))); 
				
				buffX2 =(int)( goal.x - ((width/2)*Math.sin(Math.toRadians(angle)))); 
				buffY2 =(int)( goal.y + ((width/2)*Math.cos(Math.toRadians(angle)))); 
				
				g.drawLine(points[0].x, points[0].y, buffX, buffY);
				g.drawLine(points[1].x, points[1].y, buffX2, buffY2);
				//points[0] = new Point()
				counter++;
			}*/
			g.drawLine(curX, curY, goal.x, goal.y);
			curX = goal.x;
			curY = goal.y;
		}
	}
	
    private Point frame2To1(Point p)
	{
        double newAngle = angle/180*(Math.PI);
        
        double x=p.x;
        double y=p.y;
        
        double newX = x * Math.cos(newAngle) - y * Math.sin(newAngle) + this.x;
        double newY = x * Math.sin(newAngle) + y * Math.cos(newAngle) + this.y;
        return new Point((int)newX, (int)newY);
    }

    /**
     * Convert frame given the box position (not using current default box position)
     * Used in tracing
     * @param p
     * @param bp
     * @return
     */
    private Point frame2To1(Point p, BoxPosition bp)
	{
        double newAngle = bp.angle/180*(Math.PI);
        
        double x=p.x;
        double y=p.y;
        
        double newX = x * Math.cos(newAngle) - y * Math.sin(newAngle) + bp.center.x;
        double newY = x * Math.sin(newAngle) + y * Math.cos(newAngle) + bp.center.y;
        return new Point((int)newX, (int)newY);
    }
    
    
    /**
     * Updates the four corners of the box
     * Originally from draw() method, but figured out that these points are useful
     * for pose estimation as well 
     */
    public void updateCorners()
    {
		p1 = new Point(-width/2,-height/2);
        p1 = frame2To1(p1);
        
        p2=new Point(width/2,-height/2);
        p2 = frame2To1(p2);
        
        p3=new Point(width/2,height/2);
        p3 = frame2To1(p3);

        p4=new Point(-width/2,height/2);
        p4 = frame2To1(p4);
        
        polyBounds = new Polygon();
        polyBounds.addPoint(p1.x, p1.y);
        polyBounds.addPoint(p2.x, p2.y);
        polyBounds.addPoint(p3.x, p3.y);
        polyBounds.addPoint(p4.x, p4.y);
    }

    /**
     * 
     * @param i Integer 0 to 6
     * @return
     */
    public Point getForcePoint(int i)
    {
    	double x,y;
    	
    	Line l, l2;
    	switch(i)
    	{
    		case 0:
    			l = new Line(p1,p4);
    			return l.getCenterPoint();
    		case 3:
    			l = new Line(p2,p3);
    			return l.getCenterPoint();
    		case 6:
    			l = new Line(p1,p2);
    			return l.getCenterPoint();
    		case 7:
    			l = new Line(p3,p4);
    			return l.getCenterPoint();
    		case 1:
    			l = new Line(p1,p2);
    			l2 = new Line(p1, l.getCenterPoint());
    			return l2.getCenterPoint();
    			//l = new Line(p1,p2);
    			//return l.get1ThirdsPoint();
    		case 2:
    			l = new Line(p1,p2);
    			l2 = new Line(l.getCenterPoint(),p2);
    			return l2.getCenterPoint();
    			//l = new Line(p1,p2);
    			//return l.get2ThirdsPoint();
    		case 5:
    			l = new Line(p3,p4);
    			l2 = new Line(p4, l.getCenterPoint());
    			return l2.getCenterPoint();
    			//l = new Line(p3,p4);
    			//return l.get2ThirdsPoint();
    		case 4:
    			l = new Line(p3,p4);
    			l2 = new Line(p3, l.getCenterPoint());
    			return l2.getCenterPoint();
    			//l = new Line(p3,p4);
    			//return l.get1ThirdsPoint();
    	}
    	/** Will never get here **/
    	return new Point();
    }
    
    private void calForcePara()
    {
    	if(this.numberOfRobotsToPush == 1)
    		for(int i=0; i<8;i++)
    			forceValue[i]*=2;
        
        /**
         *        p1 _______ p2
         *			|       |
         *			|       |
         *			|       |
         *     	  p3|_______|p4
         */
        
        double fx = forceValue[0] - forceValue[3];
        double fy = forceValue[1] + forceValue[2] + forceValue[6] - forceValue[4] - forceValue[5] - forceValue[7];
        netForceAngle = (Math.atan2(fy,fx)) / Math.PI * 180; //local coordinate
        netForceValue = Math.sqrt( fx*fx + fy*fy );
        netTorque = (forceValue[5] + forceValue[2] - forceValue[1] - forceValue[4]) * lArm;
    }
    
    private void calNextPosition(BoxPosition pose)
    {
    	calForcePara();
        pose.center.x = (int) (x+Mf*netForceValue*Math.cos((netForceAngle+angle)/180*Math.PI));  // global coordinate
        pose.center.y = (int) (y+Mf*netForceValue*Math.sin((netForceAngle+angle)/180*Math.PI));  // global coordinate
        pose.angle = angle+netTorque*Mt;
        pose.angle = pose.angle % 360.0;
        
        //pose.printStats();
    }

    public void boxResponse()
    {
        //save the current pose
    	BoxPosition oldPose=new BoxPosition();
        oldPose.center.x = x;
        oldPose.center.y = y;
        oldPose.angle = angle;
        
        BoxPosition newPose=new BoxPosition();
        
        if(!isEnoughForceValues())
    	{
    		//System.out.println("NOT ENOUGH FV");
    		return;
    	}
		
        /** Get the new GLOBAL coordinate for the box **/
        calNextPosition(newPose);
		x = newPose.center.x;
		y = newPose.center.y;
		angle = newPose.angle;
		
		/** Update the corners and importantly the bounds **/
		updateCorners();
		
		/** If box moves to an invalid place, reject the move! **/
		if(!this.validateBoxPoints(Main.a))
		{
			x = oldPose.center.x;
			y = oldPose.center.y;
			angle = oldPose.angle;
			
			/** NO NEED to updateCorners, done in draw()**/
		}
		/** ELSE keep the trace of old position if move was valid **/
		else
		{
			traceList.add(oldPose);
		}
		
		
		PathCalculator.validatePath(new Point(this.x, this.y), goalPath.get(0),goalPath, this, true);
		
		resetForceValues();
     }

    public boolean isEnoughForceValues()
    {
    	double sum = 0; /** sum of forces **/
    	for(int i = 0; i < 8; i++)
    	{
    		sum += forceValue[i];
    	}
    	//System.out.println("sum " + sum);
    	return sum >= (10 * this.numberOfRobotsToPush);
    }
    
    public void resetForceValues()
    {
    	for(int i = 0; i < 8; i++)
    	{
    		forceValue[i] = 0;
    	}
    }
    public void exertForce(int action, double fValue)
    {
    	forceValue[action] = forceValue[action] + fValue;
    	if(forceValue[action] > 10)
    		forceValue[action] = 10;
    }
    
	public String getID() {
		return id;
	}
	
	
	/** helper for pushToGoal **/
	public double getGoalAngle(Point goal)
	{
        Point goalLocalPosition=new Point(0,0);
        
        frameConvert(goal, goalLocalPosition, new Point(getX(),getY()), angle);
        double goalAngle=(Math.atan2(goalLocalPosition.y, goalLocalPosition.x))/Math.PI*180;
        goalAngle += 90;
        while(goalAngle<0)
            goalAngle=goalAngle+360;
		return goalAngle;
	}
	
	public double getGoalAngle2(Point goal)
	{
        Point goalLocalPosition=new Point(0,0);
        //frameconvert uses 0 angle because we don't care about box's angle
        //we just want to know the goal with respect to the box
        frameConvert(goal, goalLocalPosition, new Point(getX(),getY()), 0); 
        double goalAngle=(Math.atan2(goalLocalPosition.y, goalLocalPosition.x))/Math.PI*180;
        goalAngle += 90;
        while(goalAngle<0)
            goalAngle=goalAngle+360;
		return goalAngle;
	}
	
	
	
	public double getGoalAngle()
	{
		if(getBounds().contains(new Point(goalPath.get(0).x,goalPath.get(0).y )))
		{
			goalPath.remove(0);
			goalPath = PathCalculator.getGoalPath(this, Main.g1);
			System.err.println("NEXT GOAL: " + goalPath.get(0).x + ", " + goalPath.get(0).y);
			System.err.println("CURRENT POSITION: " + this.x + ", " + this.y);
		}
		
		Point goal = new Point(goalPath.get(0).x,goalPath.get(0).y );
				
        Point goalLocalPosition=new Point(0,0);
        
        frameConvert(goal, goalLocalPosition, new Point(getX(),getY()), angle);
        
        double goalAngle=(Math.atan2(goalLocalPosition.y, goalLocalPosition.x))/Math.PI*180;
        goalAngle += 90;
        while(goalAngle<0)
            goalAngle=goalAngle+360;
		return goalAngle;
	}
	
    private void frameConvert(Point p1, Point p2, Point origin, double angle){  // convert the global coordinate into local coordinate
        p2.x=(int) (p1.x*Math.cos(angle/180*Math.PI)+p1.y*Math.sin(angle/180*Math.PI)
             -origin.x*Math.cos(angle/180*Math.PI)-origin.y*Math.sin(angle/180*Math.PI));
        p2.y=(int) (p1.y*Math.cos(angle/180*Math.PI)-p1.x*Math.sin(angle/180*Math.PI)
             +origin.x*Math.sin(angle/180*Math.PI)-origin.y*Math.cos(angle/180*Math.PI));
    }    

	public Capabilities getCapabilities()
	{
		return this.capabilitiesNeeded;
	}	
	
	public void setCapabilities(Capabilities cap)
	{
		this.capabilitiesNeeded = cap;
	}
	
	public boolean validateBoxPoints(ArrayList<EnvObjInterface> a)
	{
		/** 
		 * Only pushing robots can touch the box! 
		 **/
		for(EnvObjInterface obj : a)
		{
			if(obj.getID().startsWith("r") && obj.getBounds().intersects(this.getBounds()))
			{
				Robot r = (Robot) obj;
				if(r.myBox == null || !this.id.equals(r.myBox.getID()))
					return false;
			}
		}
		return true;
	}
	
	public Point[] getStartingPoints()
	{
		Point points[]  = new Point[2];
		int x,y;
		
		points[0] = getForcePoint(0);
		points[1] = getForcePoint(3);
		
		return points;
		
	}

	public void setStartCoordinationBeforePushing()
	{
		if(round1 < 0)
		{
			round1 = Main.rounds;
		}
	}
	
	public void setStartBeingPushed()
	{
		if(round2 < 0)
		{
			round2 = Main.rounds;
		}
	}
	
	public void setIsInGoal() 
	{
		if(this.isPushable)
		{
			round3 = Main.rounds;
			System.out.println("Box " + id + " coordinated in " + (round2 - round1) + " TIMESTEPs, started at tstep# " + round1 + " ends at " + round2);
			System.out.println("Box " + id + " pushed in " + (round3 - round2) + " TIMESTEPs, started at tstep# " + round2 + " ends at " + round3);
			this.setPushable(false);
			Main.duringTxRounds += round3 - round2;
			Main.beforeTxRounds += round2 - round1;
			Main.numberOfBoxInGoal++;
		}
	}
}
